#include <chrono>  // 包含用于处理时间的库
#include <memory>  // 包含智能指针库
#include <string>  // 包含字符串库
#include <thread>  // 包含线程库
#include <utility> // 包含实用工具库

#include "lifecycle_msgs/msg/transition.hpp" // 包含生命周期消息中的转换消息

#include "rclcpp/rclcpp.hpp"    // 包含ROS 2客户端库
#include "rclcpp/publisher.hpp" // 包含ROS 2的发布者库

#include "rclcpp_lifecycle/lifecycle_node.hpp"      // 包含生命周期节点库
#include "rclcpp_lifecycle/lifecycle_publisher.hpp" // 包含生命周期发布者库
#include "rclcpp_action/rclcpp_action.hpp"
#include "test_interfaces/action/fibonacci.hpp"
#include "std_msgs/msg/string.hpp" // 包含标准消息中的字符串消息

using namespace std::chrono_literals; // 使用chrono库中的字面量表示法

class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode
{
public:
    using Fibonacci = test_interfaces::action::Fibonacci;
    using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

    explicit LifecycleTalker(const std::string &node_name, bool intra_process_comms = false)
        : rclcpp_lifecycle::LifecycleNode(node_name,
                                          rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms)) // 初始化父类LifecycleNode
    {
    }

    void publish()
    {
        static size_t count = 0;                                        // 静态计数器
        auto msg = std::make_unique<std_msgs::msg::String>();           // 创建一个唯一指针的字符串消息
        msg->data = "Lifecycle HelloWorld #" + std::to_string(++count); // 设置消息内容

        // 打印当前状态以供演示
        if (!pub_->is_activated())
        {
            RCLCPP_INFO(
                get_logger(), "Lifecycle publisher is currently inactive. Messages are not published."); // 如果发布者未激活，打印信息
        }
        else
        {
            RCLCPP_INFO(
                get_logger(), "Lifecycle publisher is active. Publishing: [%s]", msg->data.c_str()); // 如果发布者已激活，打印并发布信息
        }

        // 我们在任何状态下都调用生命周期发布者的发布函数。
        // 只有在发布者处于激活状态时，消息传输才会启用并且消息实际发布。
        pub_->publish(std::move(msg)); // 发布消息
    }

    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID &uuid,
        std::shared_ptr<const Fibonacci::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
        (void)uuid;
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
        using namespace std::placeholders;
        // this needs to return quickly to avoid blocking the executor, so spin up a new thread
        std::thread{std::bind(&LifecycleTalker::execute, this, _1), goal_handle}.detach();
    }

    void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing goal");
        rclcpp::Rate loop_rate(1);
        const auto goal = goal_handle->get_goal();
        auto feedback = std::make_shared<Fibonacci::Feedback>();
        auto &sequence = feedback->partial_sequence;
        sequence.push_back(0);
        sequence.push_back(1);
        auto result = std::make_shared<Fibonacci::Result>();

        for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i)
        {
            // Check if there is a cancel request
            if (goal_handle->is_canceling())
            {
                result->sequence = sequence;
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "Goal canceled");
                return;
            }
            // Update sequence
            sequence.push_back(sequence[i] + sequence[i - 1]);
            // Publish feedback
            goal_handle->publish_feedback(feedback);
            RCLCPP_INFO(this->get_logger(), "Publish feedback");

            loop_rate.sleep();
        }

        // Check if goal is done
        if (rclcpp::ok())
        {
            result->sequence = sequence;
            goal_handle->succeed(result);
            RCLCPP_INFO(this->get_logger(), "Goal succeeded");
        }
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    on_configure(const rclcpp_lifecycle::State &)
    {
        pub_ = this->create_publisher<std_msgs::msg::String>("lifecycle_chatter", 10); // 创建生命周期发布者
        timer_ = this->create_wall_timer(
            1s, [this]()
            { return this->publish(); }); // 创建定时器

        using namespace std::placeholders;
        this->action_server_ = rclcpp_action::create_server<Fibonacci>(
            this,
            "fibonacci",
            std::bind(&LifecycleTalker::handle_goal, this, _1, _2),
            std::bind(&LifecycleTalker::handle_cancel, this, _1),
            std::bind(&LifecycleTalker::handle_accepted, this, _1));

        RCLCPP_INFO(get_logger(), "on_configure() is called."); // 打印配置信息

        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    on_activate(const rclcpp_lifecycle::State &state)
    {
        // 父类方法自动在托管实体（目前是LifecyclePublisher）上过渡。
        // 这里也可以手动调用pub_->on_activate()。
        // 重写此方法是可选的，很多时候默认方法就足够了。
        LifecycleNode::on_activate(state);

        RCLCPP_INFO(get_logger(), "on_activate() is called."); // 打印激活信息

        std::this_thread::sleep_for(2s); // 休眠2秒

        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    on_deactivate(const rclcpp_lifecycle::State &state)
    {
        LifecycleNode::on_deactivate(state);

        RCLCPP_INFO(get_logger(), "on_deactivate() is called."); // 打印停用信息

        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    on_cleanup(const rclcpp_lifecycle::State &)
    {
        // 在我们的清理阶段，我们释放定时器和发布者的共享指针。这些实体不再可用，我们的节点是“干净”的。
        timer_.reset();
        pub_.reset();

        RCLCPP_INFO(get_logger(), "on cleanup is called."); // 打印清理信息

        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    on_shutdown(const rclcpp_lifecycle::State &state)
    {
        // 在我们的关闭阶段，我们释放定时器和发布者的共享指针。这些实体不再可用，我们的节点是“干净”的。
        timer_.reset();
        pub_.reset();

        RCLCPP_INFO(get_logger(), "on shutdown is called from state %s.", state.label().c_str()); // 打印关闭信息

        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; // 返回成功
    }

private:
    // 我们持有一个生命周期发布者的实例。根据生命周期节点所处的状态，此生命周期发布者可以被激活或停用。
    // 默认情况下，生命周期发布者在创建时是未激活的，必须激活才能将消息发布到ROS世界。
    std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> pub_;

    // 我们持有一个定时器实例，该定时器定期触发发布函数。
    // 在beta版本中，这是一个常规定时器。在未来版本中，将创建一个生命周期定时器，它遵循与生命周期发布者相同的生命周期管理。
    std::shared_ptr<rclcpp::TimerBase> timer_;

    rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
};